#include<iostream>
#include<opencv2/opencv.hpp>
#include<cmath>

cv::Mat background;

double height = 0;              // 运行高度
double target_height = 0;       // 目标高度
double force = 0;               // 升力
double total_force = 0;         // 合力
double m = 1;                   // 质量
double acceleration = 0;        // 加速度
double velocity = 0;            // 速度 
double err = 0;                 // 与目标高度差
double last_err = 0;            // 上一次高度差
double integration_sum = 0;     // 积分误差
double derivative = 0;          // 微分误差
double integration_thr = 1000;  // 积分误差限
double u = 0.5;                 // 阻力系数
double dt = 0.05;               // 单位时间

int kp = 0.0 , ki = 0.0  , kd = 0.0;
double Kp ,Ki ,Kd;

double pidResolve(double t_err, double kp, double ki, double kd)
{
    err = t_err;
    integration_sum += err / 100;
    if(abs(integration_sum) > integration_thr)
    {
        integration_sum = integration_thr * abs(integration_sum) / integration_sum;
    }
    derivative = err - last_err;
    last_err = err;
    double delta = kp * err + ki * integration_sum + kd * derivative;
    return delta;
}

// event鼠标事件代号，x,y鼠标坐标，flags拖拽和键盘操作的代号
void mouseEvent(int event, int x, int y, int flags, void *ustc)
{
	if (event == CV_EVENT_LBUTTONDOWN)
	{
		target_height = (800 - 1.0 * y) / 10;
	}
}

void on_trackbar_p(int ,void*){Kp=kp/100.0;}
void on_trackbar_i(int ,void*)
{
    Ki=ki/100.0;
    integration_sum = 0;
}
void on_trackbar_d(int ,void*){Kd=kd/10.0;}

int main()
{
    cv::namedWindow("img");
    cv::setMouseCallback("img", mouseEvent);       // 调用回调函数
    cv::createTrackbar( "Kp", "img", &kp, 100, on_trackbar_p );
    cv::createTrackbar( "Ki", "img", &ki, 100, on_trackbar_i );
    cv::createTrackbar( "Kd", "img", &kd, 500, on_trackbar_d );
    on_trackbar_p(0,0);
    on_trackbar_i(0,0);
    on_trackbar_d(0,0);

    while(true)
    {
        background = cv::Mat::zeros(1000, 600, CV_8UC3);
        cv::line(background, cv::Point(0, 803), cv::Point(600, 803), cv::Scalar(100, 100, 100), 6);
        cv::line(background, cv::Point(0, 800 - target_height * 10), cv::Point(600, 800 - target_height * 10), cv::Scalar(255, 255, 0), 2);
        std::string Target_Height = std::to_string(target_height);
        cv::putText(background, "Target Height: "+Target_Height, cv::Point(300, 800 - target_height * 10 + 30), cv::FONT_HERSHEY_COMPLEX, 0.6, cv::Scalar(255, 255, 0), 1, 8, false);

        std::string P = std::to_string(Kp);
        std::string I = std::to_string(Ki);
        std::string D = std::to_string(Kd);
        cv::putText(background,"Kp="+P, cv::Point(20,50),cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255,255,255),1, 8, false);
        cv::putText(background,"Ki="+I, cv::Point(20,100),cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255,255,255),1, 8, false);
        cv::putText(background,"Kd="+D, cv::Point(20,150),cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255,255,255),1, 8, false);

        force = pidResolve(target_height - height, Kp, Ki, Kd);
        total_force = force - m * 10 - u * velocity;
        acceleration = total_force / m;
        velocity += acceleration * dt;
        height += velocity * dt;

        if(height <= 0)
        {
            height = 0;
            total_force = 0;
            acceleration = 0;
            velocity = 0;
        }

        std::cout << "target_height: " << target_height << std::endl;
        std::cout << "height: " << height << std::endl;
        std::cout << "force: "  << force << std::endl;
        std::cout << "total_force: " << total_force << std::endl;
        std::cout << "acceleration: " << acceleration << std::endl;
        std::cout << "velocity: " << velocity << std::endl;
        std::cout << std::endl;

        cv::rectangle(background, cv::Point(280, 800 - height * 10 - 8), cv::Point(320, 800 - height * 10 - 3), cv::Scalar(200, 200, 200), -1);
        cv::rectangle(background, cv::Point(293, 800 - height * 10 - 11), cv::Point(307, 800 - height * 10 - 8), cv::Scalar(200, 200, 200), -1);
        cv::circle(background, cv::Point(300, 800 - height * 10 - 8), 2.5, cv::Scalar(200, 200, 200), -1);
        cv::line(background, cv::Point(275, 800 - height * 10 - 10), cv::Point(285, 800 - height * 10 - 10), cv::Scalar(255, 255, 255), 2);
        cv::line(background, cv::Point(315, 800 - height * 10 - 10), cv::Point(325, 800 - height * 10 - 10), cv::Scalar(255, 255, 255), 2);
        cv::line(background, cv::Point(283, 800 - height * 10 - 3), cv::Point(283, 800 - height * 10), cv::Scalar(200, 200, 200), 2);
        cv::line(background, cv::Point(317, 800 - height * 10 - 3), cv::Point(317, 800 - height * 10), cv::Scalar(200, 200, 200), 2);

        cv::imshow("img", background);
        char c = cv::waitKey(10);
        if(c == 'q') break;
    }

    return 0;
}